﻿#include <occmesh/api/half.h>

void Bounding::Init()
{
    m_xmin = std::numeric_limits<double>::max();
    m_ymin = std::numeric_limits<double>::max();
    m_zmin = std::numeric_limits<double>::max();
    m_xmax = -std::numeric_limits<double>::max();
    m_ymax = -std::numeric_limits<double>::max();
    m_zmax = -std::numeric_limits<double>::max();
}

void Bounding::Update(const gp_Pnt &p)
{
    m_xmin = std::min(m_xmin, p.X());
    m_ymin = std::min(m_ymin, p.Y());
    m_zmin = std::min(m_zmin, p.Z());
    m_xmax = std::max(m_xmax, p.X());
    m_ymax = std::max(m_ymax, p.Y());
    m_zmax = std::max(m_zmax, p.Z());
}

bool Bounding::Overlap(const Bounding &rhs)
{
    if (m_xmax < rhs.m_xmin || m_xmin > rhs.m_xmax)
        return false;
    if (m_ymax < rhs.m_ymin || m_ymin > rhs.m_ymax)
        return false;
    if (m_zmax < rhs.m_zmin || m_zmin > rhs.m_zmax)
        return false;
    return true;
}

HalfEdge::HalfEdge(TopoDS_Edge occEdge) : occEdge(occEdge)
{
    double u1, u2;
    auto curve = BRep_Tool::Curve(occEdge, u1, u2);

    // 保存首尾顶点
    m_startPoint = curve->Value(u1);
    m_endPoint = curve->Value(u2);

    m_bounding.Init();
    for (int i = 0; i < HalfParamData::overlapSample; i++)
        m_bounding.Update(curve->Value(u1 + (u2 - u1) * i / (HalfParamData::overlapSample - 1)));
}

bool HalfEdge::SameDirectionWithEdge() const
{
    return edge->halfEdge == this || SameDirectionWithTwin();
}

bool HalfEdge::SameDirectionWithTwin() const
{
    return m_sameDirectionWithTwin;
}

void HalfEdge::AccumulateVertex()
{
    if (SameDirectionWithEdge())
    {
        edge->startVertex->Accumulate(m_startPoint);
        edge->endVertex->Accumulate(m_endPoint);
    }
    else
    {
        edge->startVertex->Accumulate(m_endPoint);
        edge->endVertex->Accumulate(m_startPoint);
    }
}

void HalfEdge::AccumulateNormal()
{
    double first, second, prevFirst, prevSecond;
    auto curve = BRep_Tool::Curve(occEdge, first, second);
    auto prevCurve = BRep_Tool::Curve(prev->occEdge, prevFirst, prevSecond);

    gp_Pnt P;
    gp_Vec T, V;
    curve->D1(first, P, T);
    prevCurve->D1(prevSecond, P, V);

    auto N = T.Crossed(V).Normalized();
    if (SameDirectionWithEdge())
    {
        auto Ns = edge->startVertex->Normal();
        auto dir = N.Dot(Ns);
        // 确保法向一致
        if (dir < 0)
            N = -N;

        // 如果两个法向差不多才计入（取 = 在第一次计算时有意义，此时 dir 和 normal 都为
        // 0，取等确保第一个法向可以计入）
        // if (std::abs(dir) >= Ns.Magnitude() * (1 - 1e-1))
        edge->startVertex->Accumulate(N);
    }
    else
    {
        auto Ne = edge->endVertex->Normal();
        auto dir = N.Dot(Ne);
        if (dir < 0)
            N = -N;

        // if (std::abs(dir) >= Ne.Magnitude() * (1 - 1e-1))
        edge->endVertex->Accumulate(N);
    }
}

void HalfEdge::BuildPerpTangent()
{
    // 记录前后两条曲线的切向
    double prevFirst, prevSecond, nextFirst, nextSecond;
    auto prevCurve = BRep_Tool::Curve(prev->occEdge, prevFirst, prevSecond);
    auto nextCurve = BRep_Tool::Curve(next->occEdge, nextFirst, nextSecond);

    gp_Pnt P;
    gp_Vec V;

    // 切向被反转，使得起点和终点的切向方向相同
    prevCurve->D1(prevSecond, P, V);
    m_startPerpTangent = -V;
    nextCurve->D1(nextFirst, P, V);
    m_endPerpTangent = V;

    // 检查法向取值
    gp_Vec Ns{0, 0, 0}, Ne{0, 0, 0};
    if (edge->startVertex->Normal().SquareMagnitude())
        Ns = edge->startVertex->Normal().Normalized();
    if (edge->endVertex->Normal().SquareMagnitude())
        Ne = edge->endVertex->Normal().Normalized();

    // 移除法向分量
    if (SameDirectionWithEdge())
    {
        m_startPerpTangent = m_startPerpTangent.XYZ() - Ns.XYZ() * m_startPerpTangent.Dot(Ns);
        m_endPerpTangent = m_endPerpTangent.XYZ() - Ne.XYZ() * m_endPerpTangent.Dot(Ne);
    }
    else
    {
        m_startPerpTangent = m_startPerpTangent.XYZ() - Ne.XYZ() * m_startPerpTangent.Dot(Ne);
        m_endPerpTangent = m_endPerpTangent.XYZ() - Ns.XYZ() * m_endPerpTangent.Dot(Ns);
    }
}

Handle(Geom_BSplineSurface) HalfEdge::Surface() const
{
    return Handle(Geom_BSplineSurface)::DownCast(BRep_Tool::Surface(occFace));
}

gp_Pnt HalfEdge::StartPoint() const
{
    return m_startPoint;
}

gp_Pnt HalfEdge::EndPoint() const
{
    return m_endPoint;
}

gp_Vec HalfEdge::StartPerpTangent() const
{
    return m_startPerpTangent;
}

gp_Vec HalfEdge::EndPerpTangent() const
{
    return m_endPerpTangent;
}

bool HalfEdge::Overlap(HalfEdge *rhs)
{
    if (twin != nullptr || rhs->twin != nullptr)
        return false;

    if (!m_bounding.Overlap(rhs->m_bounding))
        return false;

    double u1, u2, v1, v2;
    auto c1 = BRep_Tool::Curve(occEdge, u1, u2);
    auto c2 = BRep_Tool::Curve(rhs->occEdge, v1, v2);

    // 要求首尾顶点重合
    if (m_startPoint.Distance(rhs->m_startPoint) > HalfParamData::overlapThreshold &&
        m_startPoint.Distance(rhs->m_endPoint) > HalfParamData::overlapThreshold)
        return false;
    if (m_endPoint.Distance(rhs->m_startPoint) > HalfParamData::overlapThreshold &&
        m_endPoint.Distance(rhs->m_endPoint) > HalfParamData::overlapThreshold)
        return false;

    // 为了处理一个线框恰好有 2 条边的情况，考虑一下中点
    double param;
    if (!GeomLib_Tool::Parameter(c2, c1->Value((u1 + u2) / 2), HalfParamData::overlapThreshold, param))
        return false;
    if (!GeomLib_Tool::Parameter(c1, c2->Value((v1 + v2) / 2), HalfParamData::overlapThreshold, param))
        return false;

    // 重合则判断方向，注意同时修改两个 HalfEdge 的方向
    if (m_startPoint.Distance(rhs->m_startPoint) < HalfParamData::overlapThreshold)
    {
        m_sameDirectionWithTwin = true;
        rhs->m_sameDirectionWithTwin = true;
    }
    else
    {
        m_sameDirectionWithTwin = false;
        rhs->m_sameDirectionWithTwin = false;
    }

    return true;
}

Edge::Edge(HalfEdge *halfEdge) : halfEdge(halfEdge)
{
    // 建立与半边的关系
    halfEdge->edge = this;
}

void Edge::ConnectEdge(Edge *rhs)
{
    auto start1 = halfEdge->StartPoint();
    auto end1 = halfEdge->EndPoint();

    auto start2 = rhs->halfEdge->StartPoint();
    auto end2 = rhs->halfEdge->EndPoint();

    if (start1.Distance(start2) < HalfParamData::overlapThreshold)
        rhs->startVertex = startVertex;
    else if (start1.Distance(end2) < HalfParamData::overlapThreshold)
        rhs->endVertex = startVertex;
    else if (end1.Distance(start2) < HalfParamData::overlapThreshold)
        rhs->startVertex = endVertex;
    else if (end1.Distance(end2) < HalfParamData::overlapThreshold)
        rhs->endVertex = endVertex;
}

Vertex::Vertex(Edge *edge)
{
    m_edges.push_back(edge);
}

void Vertex::Accumulate(const gp_Pnt &p)
{
    m_position = m_position.XYZ() + p.XYZ();
    m_count++;
}

void Vertex::Accumulate(const gp_Vec &n)
{
    // 法向不需要考虑累计次数，因为只需要方向，不考虑长度
    m_normal = m_normal.XYZ() + n.XYZ();
}

gp_Pnt Vertex::Point() const
{
    return m_position.XYZ() / m_count;
}

gp_Vec Vertex::Normal() const
{
    return m_normal;
}

bool Vertex::IsBoundary() const
{
    // 存疑
    return m_edges.size() == 1 || m_count != m_edges.size() * 2;
}

bool Vertex::IsNearVoid() const
{
    for (auto e : m_edges)
        if (e->halfEdge->occFace.IsNull() || e->halfEdge->twin->occFace.IsNull())
            return true;
    return false;
}

void Vertex::FindEdges()
{
    auto e = m_edges[0];
    m_edges.clear();

    auto halfEdge = e->halfEdge;
    do
    {
        m_edges.push_back(halfEdge->edge);
        halfEdge = halfEdge->twin;
        if (halfEdge == nullptr)
        {
            m_edges.clear();
            m_edges.push_back(e);
            return;
        }

        // 注意判断方向
        if (halfEdge->edge->startVertex == this)
        {
            if (halfEdge->SameDirectionWithEdge())
                halfEdge = halfEdge->prev;
            else
                halfEdge = halfEdge->next;
        }
        else
        {
            if (halfEdge->SameDirectionWithEdge())
                halfEdge = halfEdge->next;
            else
                halfEdge = halfEdge->prev;
        }

    } while (halfEdge != e->halfEdge);
}

const std::vector<Edge *> &Vertex::Edges() const
{
    return m_edges;
}

Wire::Wire(HalfEdge *halfEdge) : halfEdge(halfEdge)
{
}

void Wire::SetFace(TopoDS_Face face)
{
    auto h = halfEdge;
    do
    {
        h->occFace = face;
        h = h->next;
    } while (h != halfEdge);
}

TopoDS_Face Wire::Face() const
{
    return halfEdge->occFace;
}

std::vector<TopoDS_Face> Wire::Faces() const
{
    std::vector<TopoDS_Face> faces;
    auto h = halfEdge;
    do
    {
        if (h->twin)
            faces.push_back(h->twin->occFace);
        else
            faces.push_back(TopoDS_Face{});
        h = h->next;
    } while (h != halfEdge);
    return faces;
}

std::vector<TopoDS_Edge> Wire::Edges() const
{
    std::vector<TopoDS_Edge> edges;
    auto h = halfEdge;
    do
    {
        edges.push_back(h->occEdge);
        h = h->next;
    } while (h != halfEdge);
    return edges;
}

void Wire::RebuildWire()
{
    // 捏合 twin 和顶点，重建曲线
    auto half = halfEdge;
    std::vector<Handle(Geom_BSplineCurve)> rList;
    do
    {
        auto occEdge = half->occEdge;
        double first, second;
        auto curve = BRep_Tool::Curve(TopoDS::Edge(occEdge), first, second);

        int N = HalfParamData::commonEdgeSample;
        Handle(TColgp_HArray1OfPnt) points = new TColgp_HArray1OfPnt(1, N);
        Handle(TColStd_HArray1OfReal) parameters = new TColStd_HArray1OfReal(1, N);
        for (int i = 0; i < N; i++)
        {
            points->SetValue(i + 1, curve->Value(first + (second - first) / (N - 1) * i));
            parameters->SetValue(i + 1, first + (second - first) / (N - 1) * i);
        }

        // 如果有 twin，则平均两条线
        if (half->twin != nullptr)
        {
            auto curve = BRep_Tool::Curve(TopoDS::Edge(half->twin->occEdge), first, second);
            for (int i = 0; i < N; i++)
            {
                double t = half->SameDirectionWithTwin() ? first + (second - first) / (N - 1) * i
                                                         : second - (second - first) / (N - 1) * i;
                points->SetValue(i + 1, (points->Value(i + 1).XYZ() + curve->Value(t).XYZ()) / 2);
            }
        }

        // 检查定向，用端点累计的平均值替换掉采样端点
        auto edge = half->edge;
        if (half->SameDirectionWithEdge())
        {
            points->SetValue(1, edge->startVertex->Point());
            points->SetValue(N, edge->endVertex->Point());
        }
        else
        {
            points->SetValue(1, edge->endVertex->Point());
            points->SetValue(N, edge->startVertex->Point());
        }

        auto s = points->Value(1).XYZ();
        auto e = points->Value(N).XYZ();
        // std::cout << "start " << s.X() << " " << s.Y() << " " << s.Z() << " end " << e.X() << " " << e.Y() << " "
        //           << e.Z() << std::endl;
        // std::cout << "start count " << edge->start->count << " end count " << edge->end->count << std::endl;

        // 插值拟合边界
        GeomAPI_Interpolate interp(points, parameters, false, 1e-6);
        interp.Perform();
        auto nCurve = interp.Curve();
        rList.push_back(nCurve);

        half = half->next;
    } while (half != halfEdge);

    // 利用 wire 检测封闭性
    BRepBuilderAPI_MakeWire w;
    for (auto &c : rList)
        w.Add(BRepBuilderAPI_MakeEdge(c));

    if (!w.IsDone())
    {
        std::cout << "Wire is not done." << std::endl;
        return;
    }

    // 重新赋予 TopoDS_Edge，注意同时修改 twin
    half = halfEdge;
    for (int i = 0; i < rList.size(); i++)
    {
        half->occEdge = BRepBuilderAPI_MakeEdge(rList[i]).Edge();
        if (half->twin != nullptr)
            half->twin->occEdge = half->SameDirectionWithTwin() ? BRepBuilderAPI_MakeEdge(rList[i]).Edge()
                                                                : BRepBuilderAPI_MakeEdge(rList[i]->Reversed()).Edge();
        half = half->next;
    }
}